/**
 * @file   MiraMXS.cpp
 * @author Josep Sabater (jsabaterm@el.ub.edu)
 * @date   November, 2016
 * @version 1.0.0
 * @ingroup QMira
 * @brief  MXS system containing all arms
 *
 */

#include <cassert>
#include <sstream>
#include <iostream>

#include "MiraMXS.h"
#include "MiraP3Arm.h"

#include <MiraVector2D.h>
#include <MiraMath.h>

namespace QMira {
         
    MXS::MXS(const MiraGeometryLib::Placement2D &location,
             const MXSLens &dims,
             const unsigned int nRobots) : dims_(dims) {
        
        using namespace MiraGeometryLib;
                
        const P3Arm::Lengths len(62.5718, 50.80, 333.629, 121.5898);
        
        /* Create arms */
        const double orientArm0 = 0;
        double segDelta = 2*M_PI / nRobots;
        for (unsigned int i=0; i<nRobots; i++) {
            double orient = orientArm0 + i*segDelta;
            double x = location.pos.x() + dims.centerToArmFrameMM*std::cos(orient);
            double y = location.pos.y() + dims.centerToArmFrameMM*std::sin(orient);
            //std::ostringstream armIdStr;
            //armIdStr << "arm " << i;
            Placement2D origFrame(x, y, MiraUtilUBLib::Math::radianToDegree(orient));
            /* Arms are created pointing backward */
            robotCont_.push_back(IRobot2D::IRobot2DUniquePtr_t(new P3Arm(origFrame, len)));            


            assert( robotCont_.back()->id() == (robotCont_.size()-1) );
        }                  
    }

    const IRobot2D& MXS::getRobot(const unsigned int robotId) const {
        assert(isRobotValid_(robotId));
        return *(robotCont_[robotId]);
    }

    bool MXS::isRobotValid_(const unsigned int robotId) const{
    return !(robotCont_.empty() || (robotCont_.size() <= robotId)); }
}